import os
from ament_index_python import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node, PushROSNamespace
from launch.actions import IncludeLaunchDescription, GroupAction
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    """
    创建一个带有两个使用外部控制逻辑的机器人仿真环境
    为每个机器人配置一个外部控制器来保证姿态稳定
    验证tagfinder与odom差异来验证正确性
    """
    self_package_path = get_package_share_directory("april_tag_finder")
    rviz__file = os.path.join(self_package_path, "config", "tag_eva.rviz")

    package_path = get_package_share_directory("robot_define")
    urdf_file = os.path.join(package_path, "urdf", "robot_simple.urdf")
    with open(urdf_file, "r") as infp:
        urdf_content = infp.read()

    state_publisher_node1 = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        name="robot_state_publisher_node",
        output="screen",
        parameters=[{"robot_description": urdf_content}, {"frame_prefix": "robot1/"}],
        namespace="robot1",
    )

    odom2tf1 = Node(
        package="toolbox",
        executable="odom2tf",
        name="odom2tf",
        output="screen",
        remappings=[("odom", "sp_odom")],
        parameters=[{"tf_name": "chassis"}],
        namespace="robot1",
    )

    state_publisher_node2 = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        name="robot_state_publisher_node",
        output="screen",
        parameters=[{"robot_description": urdf_content}, {"frame_prefix": "robot2/"}],
        namespace="robot2",
    )
    odom2tf2 = Node(
        package="toolbox",
        executable="odom2tf",
        name="odom2tf",
        output="screen",
        remappings=[("odom", "sp_odom")],
        parameters=[{"tf_name": "chassis"}],
        namespace="robot2",
    )
    
    webotsSim = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            PathJoinSubstitution(
                [FindPackageShare("robot_sim"), "launch", "sim_multi.launch.py"]
            )
        )
    )
    ''' 
    # for docked robot
    webotsSim = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            PathJoinSubstitution(
                [FindPackageShare("robot_sim"), "launch", 
                 "sim_dock.launch.py"]
            )
        )
    )
    '''

    tagFinder1 = Node(
        package="april_tag_finder",
        executable="finder_auto",
        name="tag_finder",
        remappings=[
            ("camera_image", "camera/image_color"),
            ("camera_info", "camera/camera_info"),
        ],
        parameters=[
            {"debug": True},
            {"tag_size": 0.04},
            {"tag_family": "tag36h11"},
        ],
        output="screen",
        namespace="robot1",
    )

    tag2chassis1 = Node(
        package="april_tag_finder",
        executable="tag2chassis",
        name="tag2chassis",
        output="screen",
        namespace="robot1",
        parameters=[
            {"frame_prefix": "robot1"},
            {"target_prefix": "robot2"},
        ],
    )

    tagFinder2 = Node(
        package="april_tag_finder",
        executable="finder_auto",
        name="tag_finder",
        remappings=[
            ("camera_image", "camera/image_color"),
            ("camera_info", "camera/camera_info"),
        ],
        parameters=[
            {"debug": False},
            {"tag_size": 0.05},
            {"tag_family": "tag36h11"},
        ],
        output="screen",
        namespace="robot2",
    )
    state_controller1 = Node(
        package="controller",
        executable="tri_pid",
        name="controller",
        output="screen",
        parameters=[
            {"gyro_pid": [0.1, 0.5, 0.0, 100.0]},
            {"angle_pid": [5.0, 0.0, 0.0, 100.0]},
            {"speed_pid": [0.0, 0.0, 0.0, 100.0]},
            {"rotate_pid": [0.2, 1.0, 0.0, 100.0]},
        ],
        namespace="robot1",
    )
    state_controller2 = Node(
        package="controller",
        executable="tri_pid",
        name="controller",
        output="screen",
        parameters=[
            {"gyro_pid": [0.1, 0.5, 0.0, 100.0]},
            {"angle_pid": [5.0, 0.0, 0.0, 100.0]},
            {"speed_pid": [0.0, 0.0, 0.0, 100.0]},
            {"rotate_pid": [0.2, 1.0, 0.0, 100.0]},
        ],
        namespace="robot2",
    )

    r1 = GroupAction(
        actions=[
            PushROSNamespace("robot1"),
            IncludeLaunchDescription(
                PythonLaunchDescriptionSource(
                    PathJoinSubstitution(
                        [
                            FindPackageShare("april_tag_finder"),
                            "launch",
                            "detecte.launch.py",
                        ]
                    )
                ),
                launch_arguments={"prefix": "/robot1"}.items(),
            ),
        ]
    )
    r2 = GroupAction(
        actions=[
            PushROSNamespace("robot2"),
            IncludeLaunchDescription(
                PythonLaunchDescriptionSource(
                    PathJoinSubstitution(
                        [
                            FindPackageShare("april_tag_finder"),
                            "launch",
                            "detecte.launch.py",
                        ]
                    )
                ),
                launch_arguments={"prefix": "/robot2"}.items(),
            ),
        ]
    )

    rviz2_node = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="screen",
        arguments=["-d", rviz__file],
    )

    return LaunchDescription(
        [
            # state_publisher_node1,
            # odom2tf1,
            # state_publisher_node2,
            # odom2tf2,
            ## tf,
            state_controller1,
            state_controller2,
            # tagFinder1,
            ## tagFinder2
            # tag2chassis1,
            r1,
            r2,
            rviz2_node,
            webotsSim,
        ]
    )
